#include "ClientConnect.h"
#include "ThreadListen.h"
#include "Client.h"
#include "nbt.h"

#define DEBUG

#ifndef DEBUG
#define gePrintDebug(...) ;
#endif

ClientConnect::ClientConnect(Client* cli){
	client = cli;
	threadListen = new ThreadListen(client, this);
	address = NULL;
	port = 0;
	secureMode = false;
	KEYLEN = 16;
	PRIVATE_KEYLEN = 128;
	recv_buf_i = 0;
	recv_buf_size = 0;
	memset(recv_buf, 0, sizeof(recv_buf));
	memset(string_read, 0, sizeof(string_read));
	send_i = 0;
	send_buf_i[0] = 0;
	send_buf_i[1] = 0;
	send_buf_i[2] = 0;
	send_buf_i[3] = 0;
	send_buf_i[4] = 0;
	send_buf_i[5] = 0;
	send_buf_i[6] = 0;
	send_buf_i[7] = 0;
	memset(send_buf, 0, sizeof(send_buf));
	string_read_i = 0;
	connected = false;
	receiving = false;
	sending_data = false;
	sending[0] = false;
	sending[1] = false;
	sending[2] = false;
	sending[3] = false;
	sending[4] = false;
	sending[5] = false;
	sending[6] = false;
	sending[7] = false;
}

ClientConnect::~ClientConnect(){
}

bool ClientConnect::isConnected(){
	return connected;
}

void ClientConnect::setReceiving(bool recv){
	receiving = recv;
}

void ClientConnect::Join(const char* _address, int _port){
//	geDebugMode(GE_DEBUG_ALL);
	geDebugCritical(false);
	printf("Join 1\n");
	threadListen->Stop();
	printf("Join 2\n");
	if(address)free(address);
	address = strdup(_address);
	port = _port;
	printf("Join 3\n");

	socket = geCreateSocket(GE_SOCKET_TYPE_CLIENT, address, port, GE_PORT_TYPE_TCP);
	geSocketConnect(socket);
	
	printf("Join 4\n");
	Connect();
	printf("Join 5\n");
	
	threadListen->Start();
	printf("Join 6\n");
//	WaitCommand(0x01, 0);
	while(!threadListen->GetLoginCmd())geSleep(1);
	printf("Join 7\n");
	connected = true;
//	WaitCommand(0x0D, 0);
//	geSleep(500);
	SendClientSettings();
	printf("Join 8\n");
}

void ClientConnect::WaitCommand(u8 id, u32 timeout){
	u32 start = geGetTick();
	while(threadListen->GetLastCmd() != id && (timeout == 0 || (geGetTick() - start) < timeout)){
		geSleep(1);
	}
}

int ClientConnect::computeString(char* out, const char* in){
	int i, j;
	int len = strlen(in);
	((short*)out)[0] = htons((short)len);
	for(i=0, j=2; i<len; i++, j+=2){
		out[j+0] = 0x0;
		out[j+1] = in[i];
	}
	return len * 2 + 2;
}

u8* ClientConnect::DecodeX509(u8* key, int keylen, u8* token, int tokenlen, u8** tokenout){
	u8* private_crypted;
	RSA* rsa;
	srand(time(NULL));

	OpenSSL_add_all_algorithms();
	ERR_load_crypto_strings();

	// Get server's PublicKey
	rsa = d2i_RSA_PUBKEY(NULL, (const unsigned char**)&key, keylen);

	// Generate random PrivateKey
	private_key = (u8*)malloc(KEYLEN);
	private_crypted = (u8*)malloc(PRIVATE_KEYLEN);
	RAND_bytes(private_key, KEYLEN);

	// Encrypt PrivateKey and token
	*tokenout = (u8*)malloc(PRIVATE_KEYLEN);
	RSA_public_encrypt(KEYLEN, private_key, private_crypted, rsa, RSA_PKCS1_PADDING);
	RSA_public_encrypt(tokenlen, token, *tokenout, rsa, RSA_PKCS1_PADDING);

	RSA_free(rsa);
	return private_crypted;
}

void ClientConnect::CiphersInit(u8* key, u8* iv){
	EVP_CIPHER_CTX_init(&ctx_encrypt);
	EVP_EncryptInit(&ctx_encrypt, EVP_aes_128_cfb8(), key, iv);

	EVP_CIPHER_CTX_init(&ctx_decrypt);
	EVP_DecryptInit(&ctx_decrypt, EVP_aes_128_cfb8(), key, iv);

	ctx_block_size = EVP_CIPHER_block_size(EVP_aes_128_cfb8());
}

void ClientConnect::Encrypt(u8* data, int size, u8** out, int *outlen){
	*out = (u8*)geMalloc(size + ctx_block_size + 1);
    EVP_EncryptUpdate(&ctx_encrypt, *out, outlen, data, size);
}

void ClientConnect::Decrypt(u8* data, int size, u8** out, int *outlen){
	*out = (u8*)geMalloc(size + ctx_block_size + 1);
    EVP_DecryptUpdate(&ctx_decrypt, *out, outlen, data, size);
}

int ClientConnect::Send(u8* buf, int buflen){
	while(sending_data)geSleep(0);
	sending_data = true;
	int ret = 0;
	if(secureMode){
		u8* buf2 = NULL;
		int buflen2 = 0;
		Encrypt((u8*)buf, buflen, &buf2, &buflen2);
		gePrintDebug(0, "Send crypted data (%d) [0x%02X] :\n", buflen2, buf[0]);
		//gePrintBuffer(buf, buflen);
		ret = geSocketSend(socket, buf2, buflen2);
		geFree(buf2);
	}else{
		gePrintDebug(0, "Send data (%d) [0x%02X] :\n", buflen, buf[0]);
		//gePrintBuffer(buf, buflen);
		ret = geSocketSend(socket, buf, buflen);
	}
	sending_data = false;
	return ret;
}

int ClientConnect::Receive(u8* buf, int max){
	int ret = 0;
	if(secureMode){
		u8* data = NULL;
		int recv_ret = geSocketReceive(socket, buf, max);
		Decrypt((u8*)buf, recv_ret, &data, &ret);
		memcpy(buf, data, ret);
		geFree(data);
	}else{
		ret = geSocketReceive(socket, buf, max);
	}
	if(ret <= 0){
		gePrintDebug(2, "Receive error (%d [%d]) (%s)\n", ret, max, strerror(errno));
		connected = false;
		while(1)geSleep(10);
	}
	return ret;
}

u8* ClientConnect::GetReadPtr(int size){
	gePrintDebug(0, "GetReadPtr(%d)\n", size);
	if(size < 0){
		((u8*)0)[0] = 0;
	}
	if(recv_buf_i + size > recv_buf_size){
		if((int)recv_buf_size - (int)recv_buf_i > 0){
			printf("memcpy(recv_buf, &recv_buf[%d], %d-%d=%d)\n", recv_buf_i, recv_buf_size, recv_buf_i, recv_buf_size - recv_buf_i);
			memcpy(recv_buf, &recv_buf[recv_buf_i], recv_buf_size - recv_buf_i);
			recv_buf_size = recv_buf_size - recv_buf_i + Receive(&recv_buf[recv_buf_size-recv_buf_i], sizeof(recv_buf) - (recv_buf_size-recv_buf_i));
		}else{
			recv_buf_size = Receive(recv_buf, sizeof(recv_buf));
		}
		recv_buf_i = 0;
	}
	u8* ret = &recv_buf[recv_buf_i];
	recv_buf_i += size;
	return ret;
}

void ClientConnect::ReadData(u8* ret, int size){
	for(int i=0; i<size; i+=128){
		if(size - i < 128){
			memcpy(&ret[i], GetReadPtr(size - i), size - i);
		}else{
			memcpy(&ret[i], GetReadPtr(128), 128);
		}
	}
}

char ClientConnect::ReadByte(){
	u8* buf = GetReadPtr(1);
	char ret = (char)buf[0];
	return ret;
}

short ClientConnect::ReadShort(){
	u8* buf = GetReadPtr(2);
	short ret = ((short*)buf)[0];
	ret = ntohs(ret);
	return ret;
}

int ClientConnect::ReadInt(){
	u8* buf = GetReadPtr(4);
	int ret = ((int*)buf)[0];
	ret = ntohl(ret);
	return ret;
}

uint64_t ClientConnect::ReadLong(){
	u8* buf = GetReadPtr(8);
	uint64_t ret = ((uint64_t*)buf)[0];
	ret = bswap64(ret);
	return ret;
}

float ClientConnect::ReadFloat(){
	u8* buf = GetReadPtr(4);
	union {
		u32 i;
		float f;
	} f;
	f.i = bswap32(((u32*)buf)[0]);
	return f.f;
}

double ClientConnect::ReadDouble(){
	u8* buf = GetReadPtr(8);
	printf("ReadDouble::buf : ");
	gePrintBuffer(buf, 8);
	union {
		u64 i;
		double d;
	} d;
	d.i = bswap64(((u64*)buf)[0]);
	return d.d;
}

int ClientConnect::ReadString(char* out){
	int len = ReadShort();
	printf("ReadString len : 0x%08X  %d  %d\n", len, len, bswap16(len & 0xFFFF));
	u8* buf = GetReadPtr(len * 2);
	int i, j;
	for(i=0, j=0; j<len; i+=2, j++){
		out[j] = buf[i+1];
	}
	out[j] = '\0';
	return len;
}

const char* ClientConnect::GetString(){
	char* ret;
	ReadString(string_read[string_read_i]);
	ret = string_read[string_read_i];
	string_read_i = (string_read_i + 1) % 8;
	return ret;
}

void ClientConnect::ReadSlots(Slot* slots, int n){
	int i;
	u8 unused_nbt_data_for_now[8192];
	for(i=0; i<n; i++){
		slots[i].id = ReadShort();
		gePrintDebug(0, " => id[%d/%d]: %d\n", i, n-1, slots[i].id);
		if(slots[i].id >= 0){
			slots[i].count = ReadByte();
			slots[i].damage = ReadShort();
			slots[i].nbt_length = ReadShort();
			if(slots[i].nbt_length > 0){
				(void)ReadData(unused_nbt_data_for_now, slots[i].nbt_length);
			}
		}
	}
}

void ClientConnect::ReadMetadata(Metadata* metas){
	u8 x = 0;
	while((x = ReadByte()) != 127){
		int index = x & 0x1F;
		int type = x >> 5;
		metas[index].type = type;
		switch(type){
			case 0: //byte
				metas[index].byte = ReadByte();
				break;
			case 1: //short
				metas[index].shrt = ReadShort();
				break;
			case 2: //int
				metas[index].integer = ReadInt();
				break;
			case 3: //float
				metas[index].flt = ReadFloat();
				break;
			case 4: //string16
				ReadString(metas[index].string);
				break;
			case 5: //short, byte, short, short (slot type)
				ReadSlots(&metas[index].slot, 1);
				break;
			case 6: //int, int, int
				metas[index].coord[0] = ReadInt();
				metas[index].coord[1] = ReadInt();
				metas[index].coord[2] = ReadInt();
				break;
		}
		//TODO : read metadata
	}
}

int ClientConnect::SendInit(){
	send_i = (send_i + 1) % 8;
	while(sending[send_i])geSleep(10);
	sending[send_i] = true;
	gePrintDebug(0, "Sending packet...\n");
	return send_i;
}

int ClientConnect::SendFinalize(int buf_id, bool force_send){
	int ret = 0;
	int sz = send_buf_i[buf_id];
	send_buf_i[buf_id] = 0;
	if(connected || force_send){
		//while(receiving)geSleep(1);
		ret = Send(send_buf[buf_id], sz);
	}
	sending[buf_id] = false;
	gePrintDebug(0, "Send done\n\n");
	return ret;
}

void ClientConnect::SendByte(int buf_id, char b){
	sending[buf_id] = true;
	send_buf[buf_id][send_buf_i[buf_id]] = b;
	send_buf_i[buf_id] += 1;
	gePrintDebug(0, "Byte : %d\n", b);
}

void ClientConnect::SendShort(int buf_id, short s){
	sending[buf_id] = true;
	short* buf = (short*)&send_buf[buf_id][send_buf_i[buf_id]];
	buf[0] = bswap16(s);
	send_buf_i[buf_id] += 2;
	gePrintDebug(0, "Short : %d\n", s);
}

void ClientConnect::SendInt(int buf_id, int i){
	sending[buf_id] = true;
	int* buf = (int*)&send_buf[buf_id][send_buf_i[buf_id]];
	buf[0] = bswap32(i);
	send_buf_i[buf_id] += 4;
	gePrintDebug(0, "Int : %d\n", i);
}

void ClientConnect::SendLong(int buf_id, int64_t l){
	sending[buf_id] = true;
	int64_t* buf = (int64_t*)&send_buf[buf_id][send_buf_i[buf_id]];
	buf[0] = bswap64(l);
	send_buf_i[buf_id] += 8;
	gePrintDebug(0, "Long : %d\n", l);
}

void ClientConnect::SendFloat(int buf_id, float f){
	sending[buf_id] = true;
	u32* buf = (u32*)&send_buf[buf_id][send_buf_i[buf_id]];
	union {
		u32 i;
		float f;
	} data;
	data.f = f;
	buf[0] = bswap32(data.i);
	send_buf_i[buf_id] += 4;
	gePrintDebug(0, "Float : %f\n", f);
}

void ClientConnect::SendDouble(int buf_id, double d){
	sending[buf_id] = true;
	u64* buf = (u64*)&send_buf[buf_id][send_buf_i[buf_id]];
	union {
		u64 i;
		double d;
	} data;
	data.d = d;
	buf[0] = bswap64(data.i);
	send_buf_i[buf_id] += 8;
	gePrintDebug(0, "Double : %f\n", d);
}

void ClientConnect::SendString(int buf_id, const char* str){
	sending[buf_id] = true;
	int i, j;
	int len = strlen(str);
	char* out = (char*)&send_buf[buf_id][send_buf_i[buf_id]];
	((short*)out)[0] = bswap16((short)len);
	for(i=0, j=2; i<len; i++, j+=2){
		out[j+0] = 0x0;
		out[j+1] = str[i];
	}
	send_buf_i[buf_id] += len * 2 + 2;
	gePrintDebug(0, "String : '%d'\n", str);
}

void ClientConnect::Connect(){
	char addp[128] = "";
	u8 buf[16384];
	u8* public_key = NULL;
	u8* public_token = NULL;
	u8* token = NULL;
	int public_keylen = 0;
	int tokenlen = 0;
	
	SendHandshake("Mist3rAl3x", address, port);
	
	EncryptionKeyRequest(&public_key, &public_keylen, &public_token, &tokenlen);
	u8* key = DecodeX509(public_key, public_keylen, public_token, tokenlen, &token);
	CiphersInit(private_key, private_key);
	
	SendEncryptionKeyResponse(key, PRIVATE_KEYLEN, token, PRIVATE_KEYLEN);
	
	Receive(buf, 16384);
	secureMode = true;
	
	SendClientStatuses();
}

// 0x00
void ClientConnect::GetKeepAlive(){
	int id = ReadInt();
	int send_id = SendInit();
	SendByte(send_id, 0x00);
	SendInt(send_id, id);
	SendFinalize(send_id);
/*
	send_id = SendInit();
	SendByte(send_id, 0x03);
	SendString(send_id, "plop");
	SendFinalize(send_id);
*/
}

// 0x01
void ClientConnect:: GetLogin(){
	client->getPlayer()->setId(ReadInt());
	client->setLevelType(GetString());
	client->setServerMode(ReadByte());
	client->setCurrentDimension(ReadByte());
	client->setDifficulty(ReadByte());
	(void)ReadByte();
	client->setMaxPlayers(ReadByte());

#ifdef DEBUG
	printf("Player received infos :\n");
	printf("  id : %d\n", client->getPlayer()->getId());
/*
	printf("  lvl_type : \"%s\"\n", client->lvl_type);
	printf("  server_mode : %d\n", client->server_mode);
	printf("  dimension : %d\n", client->dimension);
	printf("  difficulty : %d\n", client->difficulty);
	printf("  max_players : %d\n", client->max_players);
*/
#endif
}

// 0x02
int ClientConnect::SendHandshake(const char* player, const char* address, int port){
	int send_id = SendInit();
	SendByte(send_id, 0x02);
	SendByte(send_id, 60);
	SendString(send_id, player);
	SendString(send_id, address);
	SendInt(send_id, port);
	return SendFinalize(send_id, true);
}

// 0x03
void ClientConnect::GetMessage(){
	printf("MESSAGE : '%s'\n", GetString());
}

// 0x04
void ClientConnect::TimeUpdate(){
	client->setGameTicks(ReadLong());
	client->setDayTicks(ReadLong());
}

// 0x05
void ClientConnect::EntityEquipment(){
	int id = ReadInt();
	int slotid = ReadShort();
	Slot item;
	ReadSlots(&item, 1);
	// TODO
}

// 0x06
void ClientConnect::SpawnPosition(){
	Location loc = Location((double)ReadInt(), (double)ReadInt(), (double)ReadInt());
	client->getWorld()->setSpawn(loc);
}

// 0x08
void ClientConnect::UpdateHealth(){
	client->getPlayer()->setHealth(ReadShort());
	client->getPlayer()->setFood(ReadShort());
	client->getPlayer()->setFoodSaturation(ReadFloat());
}

// 0x0A
void ClientConnect::SendPlayer(Player* p){
	int send_id = SendInit();
	SendByte(send_id, 0x0A);
	SendByte(send_id, p->isOnGround());
	SendFinalize(send_id);
}

// 0x0B
void ClientConnect::SendPlayerPosition(Player* p){
	Location loc = p->getLocation();
	int send_id = SendInit();
	SendByte(send_id, 0x0B);
	SendDouble(send_id, loc.getY());
	SendDouble(send_id, loc.getZ());
	SendDouble(send_id, loc.getZ() + 1.60);
	SendDouble(send_id, loc.getX());
	SendByte(send_id, p->isOnGround());
	SendFinalize(send_id);
}

// 0x0D
void ClientConnect::GetPlayerPositionLook(){
	double y = ReadDouble();
	double stance = ReadDouble();
	double z = ReadDouble();
	double x = ReadDouble();
	float yaw = -ReadFloat();
	float pitch = -ReadFloat();
	client->getPlayer()->setLocation(Location(x, y, z, yaw, pitch));
	client->getPlayer()->setStance(stance - z);
	client->getPlayer()->setOnGround(ReadByte());
	client->cam->x = x;
	client->cam->y = y;
	client->cam->z = stance;
	client->cam->RotH = yaw;
	client->cam->RotV = pitch;
	gePrintDebug(0, "Got position ! (%f, %f, %f)\n", x, y, z);


	
	int send_id = SendInit();
	SendByte(send_id, 0x0D);
	SendDouble(send_id, y);
	SendDouble(send_id, z);
	SendDouble(send_id, stance);
	SendDouble(send_id, x);
	SendFloat(send_id, yaw);
	SendFloat(send_id, pitch);
	SendByte(send_id, client->getPlayer()->isOnGround());
	SendFinalize(send_id);
}

// 0x0D
void ClientConnect::SendPlayerPositionLook(Player* p){
	Location loc = p->getLocation();
	int send_id = SendInit();
	SendByte(send_id, 0x0D);
	SendDouble(send_id, loc.getY());
	SendDouble(send_id, loc.getZ());
	SendDouble(send_id, p->getStance() + loc.getZ());
	SendDouble(send_id, loc.getX());
	gePrintDebug(0, "Yaw/Pitch : %f / %f\n", loc.getYaw(), loc.getPitch());
	SendFloat(send_id, -loc.getYaw());
	SendFloat(send_id, -loc.getPitch());
	SendByte(send_id, p->isOnGround());
	SendFinalize(send_id);
}

// 0x10
void ClientConnect::GetHeldItemChange(){
	client->getPlayer()->setHeldItemSlot(ReadShort());
}

// 0x14
void ClientConnect::SpawnNamedEntity(){
	int eid = ReadInt();
	const char* name = GetString();
	float y = ((float)ReadInt()) / 32.0;
	float z = ((float)ReadInt()) / 32.0;
	float x = ((float)ReadInt()) / 32.0;
	float yaw = (float)-ReadByte() * 2*M_PI / 256.0;
	float pitch = (float)ReadByte() * 2*M_PI / 256.0;
	short currentItem = std::max(0, (int)ReadShort());
	Metadata meta[32];
	ReadMetadata(meta);

	Player* p = new Player(client);
	p->setId(eid);
	p->setName((char*)name);
	Location loc = Location(x, y, z, yaw, pitch);
	loc.setMode(LOCATION_MODE_SMOOTH);
	p->setLocation(loc);
	client->getWorld()->SpawnEntity(p);
}

// 0x17
void ClientConnect::SpawnObjectVehicle(){
	int id = ReadInt();
	int type = ReadByte();
	float y = ((float)ReadInt()) / 32.0;
	float z = ((float)ReadInt()) / 32.0;
	float x = ((float)ReadInt()) / 32.0;
	float yaw = (float)-ReadByte() * 2*M_PI / 256.0;
	float pitch = (float)-ReadByte() * 2*M_PI / 256.0;
	Entity* e = client->getWorld()->SpawnObject(id, type, Location(x, y, z, yaw, pitch));
	int data = ReadInt();
	if(data != 0){
		int vy = ReadShort();
		int vz = ReadShort();
		int vx = ReadShort();
	}
}

// 0x18
void ClientConnect::SpawnMob(){
	int id = ReadInt();
	int type = ReadByte();
	float y = ((float)ReadInt()) / 32.0;
	float z = ((float)ReadInt()) / 32.0;
	float x = ((float)ReadInt()) / 32.0;
	float yaw = (float)-ReadByte() * 2*M_PI / 256.0;
	float pitch = (float)-ReadByte() * 2*M_PI / 256.0;
	float head_yaw = (float)-ReadByte() * 2*M_PI / 256.0;
	int vy = ReadShort();
	int vz = ReadShort();
	int vx = ReadShort();
	Entity* e = client->getWorld()->SpawnEntity(id, type, Location(x, y, z, yaw, pitch, head_yaw));
	e->setVelocity(((float)vx) / 32000.0, ((float)vy) / 32000.0, ((float)vz) / 32000.0);
	ReadMetadata(e->getMetadata());
}

// 0x19
void ClientConnect::SpawnPainting(){
	int id = ReadInt();
	const char* title = GetString();
	int y = ReadInt();
	int z = ReadInt();
	int x = ReadInt();
	int dir = ReadInt();
	Painting* p = (Painting*)client->getWorld()->SpawnObject(id, ENTITY_PAINTING, Location(x, y, z));
	p->setDirection(dir);
	p->setTitle(title);
}

// 0x1C
void ClientConnect::EntityVelocity(){
	int id = ReadInt();
	int vy = ReadShort();
	int vz = ReadShort();
	int vx = ReadShort();
	Entity* e = client->getWorld()->getEntityById(id);
	if(e){
	//	e->setVelocity(((float)vx) / 32000.0, ((float)vy) / 32000.0, ((float)vz) / 32000.0);
	}
}

// 0x1D
void ClientConnect::DestroyEntity(){
	int count = ReadByte();
	for(int i=0; i<count; i++){
		client->getWorld()->DestroyEntity(ReadInt());
	}
}

// 0x1F
void ClientConnect::EntityRelativeMove(){
	printf("0x1F\n");
	int id = ReadInt();
	float dy = ((float)ReadByte()) / 32.0;
	float dz = ((float)ReadByte()) / 32.0;
	float dx = ((float)ReadByte()) / 32.0;
	Entity* e = client->getWorld()->getEntityById(id);
	if(e){
		Location loc = e->getLocation();
		loc.relativeX(dx);
		loc.relativeY(dy);
		loc.relativeZ(dz);
		e->setLocation(loc);
		e->ResetForces();
	}
}

// 0x20
void ClientConnect::EntityLook(){
	printf("0x20\n");
	int id = ReadInt();
	float yaw = ((float)-ReadByte()) * 360.0 / 256.0;
	float pitch = ((float)-ReadByte()) * 360.0 / 256.0;
	Entity* e = client->getWorld()->getEntityById(id);
	
	if(e){
		Location loc = e->getLocation();
		loc.setYaw(yaw);
		loc.setPitch(pitch);
		e->setLocation(loc);
	}
}

// 0x21
void ClientConnect::EntityLookandRelativeMove(){
	printf("0x21\n");
	int id = ReadInt();
	float dy = ((float)ReadByte()) / 32.0;
	float dz = ((float)ReadByte()) / 32.0;
	float dx = ((float)ReadByte()) / 32.0;
	float yaw = ((float)-ReadByte()) * 360.0 / 256.0;
	float pitch = ((float)-ReadByte()) * 360.0 / 256.0;
	Entity* e = client->getWorld()->getEntityById(id);
	
	if(e){
		Location loc = e->getLocation();
		loc.relativeXYZ(dx, dy, dz);
		loc.setYaw(yaw);
		loc.setPitch(pitch);
		e->setLocation(loc);
	}
}

// 0x22
void ClientConnect::EntityTeleport(){
	printf("0x22\n");
	int id = ReadInt();
	float y = ((float)ReadInt()) / 32.0;
	float z = ((float)ReadInt()) / 32.0;
	float x = ((float)ReadInt()) / 32.0;
	float yaw = ((float)-ReadByte()) * 360.0 / 256.0;
	float pitch = ((float)-ReadByte()) * 360.0 / 256.0;
	Entity* e = client->getWorld()->getEntityById(id);
	
	if(e){
		Location loc = Location(x, y, z, yaw, pitch, yaw);
		loc.setMode(LOCATION_MODE_SMOOTH);
		e->setLocation(loc);
	}
}

// 0x23
void ClientConnect::EntityHeadLook(){
	int id = ReadInt();
	float head_yaw = (float)-ReadByte() * 2*M_PI / 256.0;
	Entity* e = client->getWorld()->getEntityById(id);
	printf("0x23 (%f)\n", head_yaw);
	
	if(e){
		e->getLocation().setHeadYaw(head_yaw);
	}
}

// 0x26
void ClientConnect::EntityStatus(){
	int id = ReadInt();
	int status = ReadByte();
	/*
		TODO
		2 	Entity hurt
		3 	Entity dead
		6 	Wolf taming
		7 	Wolf tamed
		8 	Wolf shaking water off itself
		9 	(of self) Eating accepted by server
		10 	Sheep eating grass
	*/
}

// 0x28
void ClientConnect::EntityMetadata(){
	Metadata fake_meta[64];
	int id = ReadInt();
	Entity* e = client->getWorld()->getEntityById(id);
	/*
	if(e){
		ReadMetadata(e->getMetadata());
	}else{
		ReadMetadata(fake_meta);
	}
	*/
	ReadMetadata(fake_meta);
}

// 0x2B
void ClientConnect::SetExperience(){
	client->getPlayer()->setExperienceBar(ReadFloat());
	client->getPlayer()->setLevel(ReadShort());
	client->getPlayer()->setExperience(ReadShort());
}

// 0x33
void ClientConnect::ChunkData(){
	int y = ReadInt() * 16;
	int x = ReadInt() * 16;
	bool continuous = ReadByte();
	u16 bitmask = ReadShort();
	u16 add_bitmask = ReadShort();
	int datalen = ReadInt();
	u8* data = (u8*)geMalloc(datalen + 1);
	ReadData(data, datalen);
	ge_Buffer* buf = geDecompress(data, datalen, GE_COMPRESSION_ZLIB);
	geFree(data);
	data = (u8*)buf->ptr;
	datalen = buf->size;
	geFree(buf);
	if(bitmask != 0 && add_bitmask != 0){
		Chunk* chunk = client->getWorld()->FindChunk(x, y);
		if(!chunk){
			chunk = client->getWorld()->ReserveChunk();
			chunk->setVisible(false);
		}
		if(chunk){
			chunk->setX(x);
			chunk->setY(y);
			chunk->LoadSMP(data, datalen, bitmask, add_bitmask, true);
			client->getThreadChunks()->EnqueueChunkBuild(chunk);
			chunk->setReserved(false);
		}
	}
	geFree(data);
}

// 0x34
void ClientConnect::MultiBlockChange(){
	int cy = ReadInt();
	int cx = ReadInt();
	int nrecs = ReadShort();
	int datalen = ReadInt();
	u8* data = (u8*)geMalloc(datalen);
	ReadData(data, datalen);
	Chunk* chunk = client->getWorld()->FindChunk(cx, cy);

	if(chunk){
		for(int i=0; i<nrecs; i++){
			u32 record = bswap32(((u32*)data)[i]);
			int meta = record & 0xF;
			int id = (record >> 4) & 0xFFF;
			int x = (record >> 24) & 0xF;
			int y = (record >> 28) & 0xF;
			int z = (record >> 16) & 0xFF;
			Block* block = chunk->GetBlock(x, y, z);
			block->setId(id);
			block->setData(meta);
		}
		client->getThreadChunks()->EnqueueChunkBuild(chunk);
	}

	geFree(data);
}

// 0x35
void ClientConnect::BlockChange(){
	int y = ReadInt();
	int z = (u8)ReadByte();
	int x = ReadInt();
	int id = ReadShort();
	int data = (u8)ReadByte();
	Chunk* chunk = client->getWorld()->FindChunk(x, y);

	if(chunk){
		Block* block = chunk->GetBlock(x & 0xF, y & 0xF, z & 0xFF);
		int last_id = block->getId();
		block->setId(id);
		block->setData(data);
		if(!(last_id == 10 && id == 11)){
			client->getThreadChunks()->EnqueueChunkBuild(chunk);
		}
	}
}

// 0x36
void ClientConnect::BlockAction(){
	int y = ReadInt();
	int z = ReadShort();
	int x = ReadInt();
	int byte1 = ReadByte();
	int byte2 = ReadByte();
	int block_id = ReadShort();
}

// 0x38
void ClientConnect::MapChunkBulk(){
	int nChunks = ReadShort();
	int datalen = ReadInt();
	bool sky_light = ReadByte();
#ifdef DEBUG
	gePrintDebug(0, "nChunks : %d\n", nChunks);
	gePrintDebug(0, "datalen : %d\n", datalen);
	gePrintDebug(0, "sky_light : %s\n", sky_light ? "true" : "false");
#endif
	u8* data = (u8*)geMalloc(datalen + 1);
	ReadData(data, datalen);
	ge_Buffer* buf = geDecompress(data, datalen, GE_COMPRESSION_ZLIB);
	geFree(data);
	data = (u8*)buf->ptr;
	datalen = buf->size;
	geFree(buf);
	u8* metadata = (u8*)geMalloc(12 * nChunks);
	ReadData(metadata, 12*nChunks);
	u8* meta = metadata;
	int i, j;
	for(i=0, j=0; i<nChunks; i++, meta+=12){
		Chunk* chunk = client->getWorld()->FindChunk(16 * bswap32(((int*)meta)[1]), 16 * bswap32(((int*)meta)[0]));
		if(!chunk){
			chunk = client->getWorld()->ReserveChunk();
			chunk->setVisible(false);
		}
		u16 bitmask = bswap16(((u16*)meta)[4]);
		u16 add_bitmask = bswap16(((u16*)meta)[5]);
		int sz = 0;
		for(int m=0; m<16; m++){
			if(bitmask & (1 << m)){
				sz += (16 * 16 * 16);		// ID
				sz += (16 * 16 * 16) / 2;	// data
				sz += (16 * 16 * 16) / 2;	// light
				if(sky_light){
					sz += (16 * 16 * 16) / 2; // sky-light
				}
			}
			if(add_bitmask & (1 << m)){
				sz += (16 * 16 * 16) / 2;	// Add array
			}
		}
		sz += 256;	// biome info
		if(j + sz > datalen){
			break;
		}
		if(chunk){
			chunk->setX(16 * bswap32(((int*)meta)[1]));
			chunk->setY(16 * bswap32(((int*)meta)[0]));
			gePrintDebug(0, "Calling Chunk::LoadSMP(j=%d, sz=%d) [datalen=%d]\n", j, sz, datalen);
			chunk->LoadSMP(&data[j], sz, bitmask, add_bitmask, sky_light);
			client->getThreadChunks()->EnqueueChunkBuild(chunk);
		}
		j += sz;
	}
	geFree(data);
	geFree(metadata);
}

// 0x3E
void ClientConnect::NamedSoundEffect(){
	const char* name = GetString();
	float y = ((float)ReadInt()) / 8.0;
	float z = ((float)ReadInt()) / 8.0;
	float x = ((float)ReadInt()) / 8.0;
	int volume = (int)(ReadFloat() * 100.0);
	int pitch = ReadByte() * 100 / 64;
	// TODOs
}

// 0x46
void ClientConnect::ChangeGameState(){
	(void)ReadByte(); //Reason
	client->getPlayer()->setGameMode(ReadByte());
}

// 0x67
void ClientConnect::GetSlot(){
	u8 win_id = ReadByte();
	int slot_id = ReadShort();
	Slot slot;
	ReadSlots(&slot, 1);
	// TODO
}

// 0x68
void ClientConnect::GetWindowItems(){
	u8 id = ReadByte();
	int nslots = ReadShort();
	if(nslots > 0){
		Slot* slots = (Slot*)geMalloc(sizeof(Slot) * nslots);
		ReadSlots(slots, nslots);
		// TODO
	}
}

// 0x82
void ClientConnect::GetUpdateSign(){
	int y = ReadInt();
	int z = ReadShort();
	int x = ReadInt();
	const char* line1 = GetString();
	const char* line2 = GetString();
	const char* line3 = GetString();
	const char* line4 = GetString();
	Chunk* chunk = client->getWorld()->FindChunk(x, y);
	if(chunk){
		chunk->UpdateSign(x, y, z, line1, line2, line3, line4);
	}
}

// 0x84
void ClientConnect::UpdateTileEntity(){
	int y = ReadInt();
	int z = ReadShort();
	int x = ReadInt();
	int action = ReadByte();
	int datalen = ReadShort();
	if(datalen > 0){
		u8* data = (u8*)geMalloc(datalen);
		ReadData(data, datalen);
		geFree(data);
	}
	//TODO
}

// 0xC9
void ClientConnect::PlayerListItem(){
	const char* name = GetString();
	bool connected = ReadByte();
	short ping = ReadShort();
	if(connected){
		client->addPlayerList(name);
		client->setPlayerPing(name, ping);
	}else{
		client->setPlayerPing(name, 0);
		client->removePlayerList(name);
	}
}

// 0xCA
void ClientConnect::GetPlayerAbilities(){
	u8 flags = ReadByte();
	client->getPlayer()->setInvulnerable(flags & 8);
	client->getPlayer()->setCanFly(flags & 4);
	client->getPlayer()->setFlying(flags & 2);
	client->getPlayer()->setCreative(flags & 1);
	client->getPlayer()->setFlyingSpeed(ReadByte());
	client->getPlayer()->setWalkingSpeed(ReadByte());

#ifdef DEBUG
	printf("Player abilities :\n");
	printf("  invulnerable : %d\n", client->getPlayer()->isInvulnerable());
	printf("  can_fly : %d\n", client->getPlayer()->canFly());
	printf("  flying : %d\n", client->getPlayer()->isFlying());
	printf("  creative : %d\n", client->getPlayer()->isCreative());
	printf("  fly speed : %d\n", client->getPlayer()->FlyingSpeed());
	printf("  walk speed : %d\n", client->getPlayer()->WalkingSpeed());
#endif
}

// 0xCC
void ClientConnect::SendClientSettings(){
	int send_id = SendInit();
	SendByte(send_id, 0xCC);
	SendString(send_id, "en_US");
	SendByte(send_id, 0);
	SendByte(send_id, 8);
	SendByte(send_id, 0);
	SendByte(send_id, true);
	SendFinalize(send_id, true);
}

// 0xCD
int ClientConnect::SendClientStatuses(){
	u8 buf[2] = { 0xCD, 0x00 };
	int ret = Send(buf, 2);
	return ret;
}

// 0xFA
void ClientConnect::GetPluginMessage(){
	const char* channel = GetString();
	int datalen = ReadShort();
	u8* data = (u8*)geMalloc(datalen);
	ReadData(data, datalen);
	//TODO
	geFree(data);
}

// 0xFC
int ClientConnect::SendEncryptionKeyResponse(u8* key, int keylen, u8* token, int tokenlen){
	u8* buf = (u8*)geMalloc(5 + keylen + tokenlen);
	int i = 0;

	buf[i] = 0xFC;
	i++;

	((short*)(&buf[i]))[0] = htons(keylen);
	memcpy(&buf[i + 2], key, keylen);
	i += 2 + keylen;

	((short*)(&buf[i]))[0] = htons(tokenlen);
	memcpy(&buf[i + 2], token, tokenlen);
	i += 2 + tokenlen;

	int ret = Send(buf, i);
	geFree(buf);
	return ret;
}

// 0xFD
void ClientConnect::EncryptionKeyRequest(u8** keyp, int* keylenp, u8** tokenp, int* tokenlenp){
	char server_id[128] = "";
	short keylen = 0;
	short tokenlen = 0;
	u8* key = NULL;
	u8* token = NULL;

	(void)ReadByte();

	ReadString(server_id);
	keylen = ReadShort();
	key = (u8*)geMalloc(keylen);
	ReadData(key, keylen);

	tokenlen = ReadShort();
	token = (u8*)geMalloc(tokenlen);
	ReadData(token, tokenlen);

#ifdef DEBUG
	printf("Server id: '%s'\n", server_id);
	printf("Public key length: %d\n", keylen);
	printf("Token length: %d\n", tokenlen);
#endif

	*keyp = key;
	*tokenp = token;
	*keylenp = keylen;
	*tokenlenp = tokenlen;
}

// 0xFF
void ClientConnect::ServerError() {
	gePrintDebug(0x102, "Server : '%s'\n", GetString());
}
